package estrategia;

import java.util.ArrayList;

import javax.vecmath.Point3d;

import comunicacion.Conector;

public class Historial {
	private static int TAMANIO_MAXIMO = 10;
	
	private Utiles utiles;
	//private Conector entorno;
	
	private ArrayList<Point3d> posiciones;
	
	public Historial(Conector entorno) {
		utiles = new Utiles(entorno);
		//this.entorno = entorno; 
		
		posiciones = new ArrayList<Point3d>();
	}
	
	public void agregar(Point3d ultimaPosicion) {
		posiciones.add(ultimaPosicion);
		
		if (posiciones.size() > TAMANIO_MAXIMO) {
			posiciones.remove(0);
		}
	}
	
	
	public Point3d getUltimaPosicion() {
		return posiciones.get(posiciones.size()-1);
	}
	
	public double getVelocidadPromedio() {
		int intervalos = posiciones.size() - 1;
		
		if (intervalos < 1) {
			return 0;
		} 
		
		int distanciaAcumulada = 0;
		
		for (int i=1; i<posiciones.size(); i++) {
			// No consideramos el intervalo de tiempo porque podemos considerarlo fijo
			double distanciaIntervaloActual = utiles.distanciaEuclidea(posiciones.get(i-1), posiciones.get(i));
			
			distanciaAcumulada += distanciaIntervaloActual;
		}
		
		// En lugar de considerar tiempo, que de hecho es constante, consideramos "pasos"
		double tiempoTotal = (double)intervalos;
		double velocidadPromedio = distanciaAcumulada / tiempoTotal;
		
		return velocidadPromedio;
	}
	
	public double getRotacionPromedio() {
		int cantMedidas = posiciones.size();
		
		if (cantMedidas < 1) {
			return 0;
		} 
		
		int rotacionAcumulada = 0;
		
		for (int i=0; i<posiciones.size(); i++) {
			double rotacionActual = posiciones.get(i).z;
			
			rotacionAcumulada += rotacionActual;
		}	
		
		double rotacionPromedio = rotacionAcumulada / (double)cantMedidas;
		
		return rotacionPromedio;
	}
}
